﻿// opencv.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
//读取图片并显示
#include <iostream>
#include <opencv2/opencv.hpp>
#include "stdio.h"
#include "open.h"
using namespace cv;
typedef unsigned char uint8;
Mat image;
Mat binary;
int left_line[96] = {0};
int left_line_flag[96] = { 0 };
int right_line[96] = { 0 };
int right_line_flag[96] = { 0 };
int mid_line[96] = { 0 };
int BlackEndL = 0;
int BlackEndM = 0;
int BlackEndR = 0;
int mend = 0;
int lend = 0;
int rend = 0;
int endline = 0;
int X, Y;
int bin_image[96][564];


void draw_point(Mat& img, int X, int Y, enum color what)
{
	if (what == 1)
	{
		img.at<Vec3b>(X, Y)[0] = 0; // 蓝色通道值
		img.at<Vec3b>(X, Y)[1] = 0; // 绿色通道值
		img.at<Vec3b>(X, Y)[2] = 255; // 红色通道值
	}
	if (what == 2)
	{
		img.at<Vec3b>(X, Y)[0] = 0; // 蓝色通道值
		img.at<Vec3b>(X, Y)[1] = 255; // 绿色通道值
		img.at<Vec3b>(X, Y)[2] = 0; // 红色通道值
	}
	if (what == 3)
	{
		img.at<Vec3b>(X, Y)[0] = 255; // 蓝色通道值
		img.at<Vec3b>(X, Y)[1] = 0; // 绿色通道值
		img.at<Vec3b>(X, Y)[2] = 0; // 红色通道值
	}
}

void search_down_line()
{
	if (bin_image[95][282] == 1)
	{
		for (Y = 282; Y > 0; Y--)
		{
			if (bin_image[95][Y] == 0 && bin_image[95][Y - 1] == 0 && bin_image[95][Y + 1] == 1)
			{
				left_line[95] = Y;
				left_line_flag[95] = 1;
				break;
			}
		}
		
	}
	else
	{
		for (Y = 282; Y < 564; Y++)
		{
			if (bin_image[95][Y] == 0 && bin_image[95][Y + 1] == 1)
			{
				left_line[95] = Y;
				left_line_flag[95] = 1;
				break;
			}
		}
		
	}
	if (left_line_flag[95] == 0)
	{
		left_line[95] = 1;
		left_line_flag[95] = 0;
	}
	if (bin_image[95][282] == 1)
	{
		for (Y = 282; Y < 564; Y++)
		{
			if (bin_image[95][Y] == 0 && bin_image[95][Y - 1] == 1 && bin_image[95][Y + 1] == 0)
			{
				right_line[95] = Y;
				right_line_flag[95] = 1;
				break;
			}
		}
	}
	else
	{
		for (Y = 282; Y > 0; Y--)
		{
			if (bin_image[95][Y] == 0 && bin_image[95][Y - 1] == 1)
			{
				right_line[95] = Y;
				right_line_flag[95] = 1;
				break;
			}
		}
	}
	
	if (right_line_flag[95] == 0)
	{
		right_line[95] = 563;
		right_line_flag[95] = 0;
	}
	
	mid_line[95] = (left_line[95] + right_line[95]) / 2;
	draw_point(binary, 95, mid_line[95] / 3, red);
	draw_point(binary, 95, left_line[95] / 3, red);
	draw_point(binary, 95, right_line[95] / 3, red);
}

void find_all()
{
	//保存二值化并检测直线后的图像
	int Lstart = 0, L_max = 0, L_min = 0;
	int Rstart = 0, R_max = 0, R_min = 0;
	int range = 10;
	for (X = 94; X > endline; X--)
	{
		if (left_line_flag[X + 1] == 0)
		{
			Lstart = 3;
			L_max = 282;
			L_min = 2;
		}
		else//如果上一行未丢线,按照跟踪巡线的方式巡线
		{
			Lstart = left_line[X + 1];
			L_max = left_line[X + 1] + range;
			L_min = left_line[X + 1] - range;
			if (L_max > 564 - 1)
			{
				L_max = 564 - 2;
			}
			if (L_min < 0)
			{
				L_min = 2;
			}
		}
		
		//左边线
		if (bin_image[X][Lstart] == 0)
		{
			for (Y = Lstart; Y < L_max; Y++)
			{
				if (bin_image[X][Y] == 0 && bin_image[X][Y + 1] == 1)
				{
					left_line[X] = Y;
					left_line_flag[X] = 1;
					break;
				}
			}
		}
		else//向左找 白->黑 跳变    左弯成立 左急弯成立
		{
			for (Y = L_max; Y > L_min; Y--)
			{
				if (bin_image[X][Y] == 1 && bin_image[X][Y - 1] == 0)
				{
					left_line[X] = Y;
					left_line_flag[X] = 1;
					break;
				}
			}
		}
		if (left_line_flag[X] == 0)
		{
			left_line[X] = 1;
		}
		

		//右边线
		//确定寻线范围
		if (right_line_flag[X + 1] == 0)
		{

			Rstart = 564 - 3;
			R_max = 564 - 2;
			R_min = 282;
		}
		else
		{
			Rstart = right_line[X + 1];
			
			R_max = right_line[X + 1] + range;
			R_min = right_line[X + 1] - range;
			if (R_max > 564 - 1)
			{
				R_max = 564 - 2;
			}

			if (R_min < 0)
			{
				R_min = 2;
			}
		}
		//右边线
		if (bin_image[X][Rstart] == 0)
		{
			//向左找
			for (Y = Rstart; Y > R_min; Y--)
			{
				if (bin_image[X][Y] == 0 && bin_image[X][Y - 1] == 1)
				{
					right_line[X] = Y;
					right_line_flag[X] = 1;
					break;
				}
			}
		}
		else
		{
			//向右找
			for (Y = R_min; Y < R_max; Y++)
			{
				if (bin_image[X][Y] == 1 && bin_image[X][Y + 1] == 0)
				{
					right_line[X] = Y;
					right_line_flag[X] = 1;
					break;
				}
			}

		}
		//如果还没找到
		if (right_line_flag[X] == 0)
		{
			right_line[X] = 562;
		}
		//巡线错误
		if (left_line[X] > right_line[X])
		{
			left_line[X] = 1;
			right_line[X] = 563;
			left_line_flag[X] = 0;
			right_line_flag[X] = 0;
		}
		//车最远一行按上一行补
		left_line[0] = left_line[1];
		left_line_flag[0] = left_line_flag[1];
		right_line[0] = right_line[1];
		right_line_flag[0] = right_line_flag[1];
		
		if (right_line_flag[X] == 1 && left_line_flag[X] == 1)
		{
			mid_line[X] = (left_line[X] + right_line[X]) / 2;
			draw_point(binary, X, mid_line[X] / 3, red);
			draw_point(binary, X, right_line[X] / 3, red);
			draw_point(binary, X, left_line[X] / 3, red);
		}
		if (left_line_flag[X] == 1 && right_line_flag[X] == 0)
		{
			mid_line[X] = (left_line[X] + right_line[X]) / 2;
			draw_point(binary, X, mid_line[X] / 3, red);
			draw_point(binary, X, left_line[X] / 3, red);
		}
		if (left_line_flag[X] == 0 && right_line_flag[X] == 1)
		{
			mid_line[X] = (left_line[X] + right_line[X]) / 2;
			draw_point(binary, X, mid_line[X] / 3, red);
			draw_point(binary, X, right_line[X] / 3, red);
		}
	}
}



void find_cross()
{
	int lost_r_d = 0, lost_r_u = 0;
	int lost_l_d = 0, lost_l_u = 0;
	float kr = 0, kl = 0;
	//右边
	for (X = image.rows - 5; X > 10; X--)
	{
		if (right_line_flag[X] == 1 && right_line_flag[X - 1] == 0)
		{
			lost_r_d = X + 2;
			printf("右下拐点:%d,%d\n", X, right_line[lost_r_d]);
			break;
		}
	}
	if (lost_r_d)
	{
		for (X = endline; X < lost_r_d - 5; X++)
		{
			if (right_line_flag[X] == 1 && right_line_flag[X + 1] == 0)
			{
				lost_r_u = X - 2;
				printf("右上拐点:%d,%d\n", X, right_line[lost_r_u]);
				break;
			}
		}
	}
	if (lost_r_d && lost_r_u)
	{
		kr = 1.01*(right_line[lost_r_d] - right_line[lost_r_u]) / (lost_r_d - lost_r_u);
		for (X = lost_r_d; X > lost_r_u; X--)
		{
			right_line[X] = (int)(right_line[lost_r_d] - (lost_r_d-X)*kr);
			draw_point(binary, X, right_line[X] / 3, red);
		}
	}
	//左边
	for (X = image.rows - 5; X > 10; X--)
	{
		if (left_line_flag[X] == 1 && left_line_flag[X - 1] == 0&& left_line_flag[X+1] == 1)
		{
			lost_l_d = X + 2;
			printf("左下拐点:%d,%d\n", X, left_line[lost_l_d]);
			break;
		}
	}
	if (lost_l_d)
	{
		for (X = endline; X < lost_l_d - 5; X++)
		{
			if (left_line_flag[X] == 1 && left_line_flag[X + 1] == 0)
			{
				lost_l_u = X - 2;
				printf("左上拐点:%d,%d\n", X, left_line[lost_l_u]);
				break;
			}
		}
	}
	if (lost_l_d && lost_l_u)
	{
		kl = 1.01 * (left_line[lost_l_d] - left_line[lost_l_u]) / (lost_l_d - lost_l_u);
		for (X = lost_l_d; X > lost_l_u; X--)
		{
			left_line[X] = (int)(left_line[lost_l_d] + (X-lost_l_d) * kl);
			draw_point(binary, X, left_line[X] / 3, red);
		}
	}
	if (lost_l_d && lost_l_u && lost_r_d && lost_r_u)
	{
		for (X = lost_l_d; X > lost_l_u; X--)
		{
			mid_line[X] = (left_line[X] + right_line[X]) / 2;
			draw_point(binary, X, mid_line[X] / 3, red);
		}
	}
}

int max(int a, int b, int c)
{
	int max;
	max = a;
	if (b > max) {
		max = b;
	}
	if (c > max) {
		max = c;
	}
	return max;
}

//截止行
void get_route_endline(void)
{
	for (X = 90; X > 0; X--)
	{
		if (bin_image[X][282] == 1&&!mend)
		{
			BlackEndM++;
		}
		else if (bin_image[X - 1][282] == 0 && bin_image[X - 2][282] == 0)
		{
			mend = 1;
		}
		if (bin_image[X][141] && !lend)
		{
			BlackEndL++;
		}
		else if (bin_image[X - 1][141] == 0 && bin_image[X - 2][141] == 0)
		{
			lend = 1;
		}
		if (bin_image[X][423] && !rend)
		{
			BlackEndR++;
		}
		else if (bin_image[X - 1][423] == 0 && bin_image[X - 2][423] == 0)
		{
			rend = 1;
		}
	}
	endline = 95- max(BlackEndL, BlackEndM, BlackEndR);
	if (endline >= 95)
	{
		endline = 95;
	}
	if (endline <= 0)
	{
		endline = 0;
	}
	for (Y = 0; Y < 564; Y++)
	{
		draw_point(binary, endline, Y / 3, blue);
	}
}

int main(int argc, char** argv)
{
	namedWindow("Line", WINDOW_NORMAL);
	resizeWindow("Line", 800, 600);
    // 加载图像
	image = imread("E:\\road\\Road\\5.BMP");
	if (image.empty()) {
	printf("Cannot open image file\n");
	return -1;
	}
	threshold(image, binary, 100, 255, THRESH_BINARY);

	//存储图像值
	for (X = 0; X < image.rows; X++)
	{
		
		for (Y = 0; Y < 3*image.cols; Y++)
		{
			bin_image[X][Y] = binary.at<uchar>(X, Y);
			if (bin_image[X][Y] == 255)
			{
				bin_image[X][Y] = 1;
			}
		}
	}
	get_route_endline();
	search_down_line();
	find_all();
	find_cross();
	imshow("Detected Lines", binary);
	imshow("Line", binary);
	waitKey(0);
    return 0;
}


